/****************************************************************************
*  Title: imageProcessor.sc
*  Author: Bryant Smith
*  Date: 04/16/2011
*  Description: takes in an 8bpp grayscale image array row by row over input 
*               c_queue
*               performs harris corner detection, sent to matcher for stereo
*               matching and hough transform of edge for lane detection
*               and uses a simple alogrythm for edge detection then returns
*				the image to the an output c_queue once edge detection is 
*				performed
*				to demonstarte system design more so than quality code for 
*               image processing
****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


// define image size, idea being from a hw camera this would be fixed
#define IMG_ROWS 480
#define IMG_CLMS 640

import "c_queue";
import "laneDetector";
import "cornerDetector";

/*
* Receives imput image from c_queue
*/
behavior receiveImage(i_receiver InQueue, out unsigned char image_buffer[IMG_ROWS][IMG_CLMS]) 
{
	// image buffers these are currently dependent on the size and format of the image used
	unsigned char row_buffer_1[IMG_CLMS];
	int row, a, j;

	void main(void) 
	{				
		
		//********************
		// Load image from c_queue into memory
		//********************
		
		// read image from c_queue and load into image buffer
		for(row = 0; row < IMG_ROWS; row++) {

			// Load row with a row of the image
			InQueue.receive(row_buffer_1, (IMG_CLMS));	
			
			// place row into image buffer
			for(a = 0; a < (IMG_CLMS); a++){
				image_buffer[row][a] =  row_buffer_1[a]; 	
			}			
			
			// generate grayscale image matrix from color RGM input image, no longer used
//			for(j = 0; j < (IMG_CLMS); j = j++)
//			{
//				// create 8 bpp grayscale image with weight of .3R + .59G + .11B
//				image8bpp[row][j/3] = (unsigned char) (0.3 * (row_buffer_1[j]) + 0.59 * (row_buffer_1[j+1]) + 0.11 * (row_buffer_1[j+2]));
//			}

		}		
		printf("Image Processor - Finished reading image into memory.  Starting Image Processing . . . .\n");
		
	}
};


/*
* smooth image with gausian filter as part of the image pre-processing
*/
behavior preProcessImageL( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out unsigned char outImage[IMG_ROWS][IMG_CLMS],
		i_sender LaneInQueue)
{
	// smoothing
	int masksize = 5;
	int gausMask[5][5] = {{2, 4, 5, 4, 2}, {4, 9, 12, 9, 4}, {5, 12, 15, 12, 5}, {4, 9, 12, 9, 4}, {2, 4, 5, 4, 2}};
	//int masksize = 3;
	//int gausianMask[3][3] = {{-1, 0, 1}, {-1, 0, 1}, {-1, 0, 1}};//{{1, 2, 1}, {2, 4, 2}, {1, 2, 1}};
	int x, y, x_1, y_1, imPosx, imPosy;
	int width, height;	
	double val;
	double image_smooth[IMG_ROWS][IMG_CLMS];
	unsigned char pixel;
	unsigned char rowBuffer[IMG_CLMS];
	
	// normalization
	int i, j, row, wt_cnt;
	double min, max, normFact;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// convolution
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				val = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						val += ((image8bpp[imPosy][imPosx]) * gausMask[y_1][x_1]);
					}
				}
				
				// should be able to apply gausian with factor such that normizalation is not required
				image_smooth[y][x] =  abs(val);  // map smoothed image
			}
		}

		//********************
		// normalization  -  
		// this should not be necessary if filter is correctly applied such that the value
		// does not exceed 255, usefull for now, not mapped to seperate behvior should be fixed.
		//********************
		// find min an max value
		max = 0;
		min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if ( image_smooth[i][j] > max) max =  image_smooth[i][j];
				if ( image_smooth[i][j] < min) min =  image_smooth[i][j];
			}
		}
		
		// determine normizalation factor
		normFact = (max - min) / 255;

		for(row = 0; row < IMG_ROWS; row++)
		{			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				pixel = (unsigned char) (( (image_smooth[row][wt_cnt]) - min) / normFact);       
				rowBuffer[wt_cnt] = pixel;// because can access output port
				outImage[row][wt_cnt] = pixel;
			}
			// send over queue for lane detection
			LaneInQueue.send(rowBuffer, (IMG_CLMS));
		}
	}
};


/*
* smooth image with gausian filter as part of the image pre-processing
*/
behavior preProcessImageR( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out unsigned char outImage[IMG_ROWS][IMG_CLMS])
{
	// smoothing
	int masksize = 5;
	int gausMask[5][5] = {{2, 4, 5, 4, 2}, {4, 9, 12, 9, 4}, {5, 12, 15, 12, 5}, {4, 9, 12, 9, 4}, {2, 4, 5, 4, 2}};
	//int masksize = 3;
	//int gausianMask[3][3] = {{-1, 0, 1}, {-1, 0, 1}, {-1, 0, 1}};//{{1, 2, 1}, {2, 4, 2}, {1, 2, 1}};
	int x, y, x_1, y_1, imPosx, imPosy;
	int width, height;	
	double val;
	double image_smooth[IMG_ROWS][IMG_CLMS];
	unsigned char pixel;
	unsigned char rowBuffer[IMG_CLMS];
	
	// normalization
	int i, j, row, wt_cnt;
	double min, max, normFact;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// convolution
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				val = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						val += ((image8bpp[imPosy][imPosx]) * gausMask[y_1][x_1]);
					}
				}
				
				// should be able to apply gausian with factor such that normizalation is not required
				image_smooth[y][x] =  abs(val);  // map smoothed image
			}
		}

		//********************
		// normalization  -  
		// this should not be necessary if filter is correctly applied such that the value
		// does not exceed 255, usefull for now, not mapped to seperate behvior should be fixed.
		//********************
		// find min an max value
		max = 0;
		min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if ( image_smooth[i][j] > max) max =  image_smooth[i][j];
				if ( image_smooth[i][j] < min) min =  image_smooth[i][j];
			}
		}
		
		// determine normizalation factor
		normFact = (max - min) / 255;

		for(row = 0; row < IMG_ROWS; row++)
		{			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				pixel = (unsigned char) (( (image_smooth[row][wt_cnt]) - min) / normFact);       
				//rowBuffer[wt_cnt] = pixel;// because can access output port
				outImage[row][wt_cnt] = pixel;
			}
			// send over queue for lane detection
			//LaneInQueue.send(rowBuffer, (IMG_CLMS));
		}
	}
};


/*
* Calculate image differentials
*/
behavior imageDiff( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out double diffxy[IMG_ROWS][IMG_CLMS])
{

	int masksize = 3;
	int convolveX[3][3] = {{-1, 0, 1}, {-1, 0, 1}, {-1, 0, 1}};
	int convolveY[3][3] = {{-1, -1, -1}, {0, 0, 0}, {1, 1, 1}};
	int x, y, x_1, y_1, posx, posy, imPosx, imPosy;
	int width, height;

	double diffx[IMG_ROWS][IMG_CLMS];
	double diffy[IMG_ROWS][IMG_CLMS];		
	double valx, valy;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// Apply x, y, xy gradient convolution
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				valx = 0;
				valy = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						valx += ((image8bpp[imPosy][imPosx]) * convolveX[y_1][x_1]);
						valy += ((image8bpp[imPosy][imPosx]) * convolveY[y_1][x_1]);
					}
				}
				diffx[y][x] = valx * valx;  // horizontial gradient
				diffy[y][x] = valy * valy;  // vertical gradient
				diffxy[y][x] =  valx * valy;  // xy gradient
			}
		}	
	}
};
